#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S4,     IRseeker,       sensorHiTechnicIRSeeker1200)
#pragma config(Motor,  mtr_S1_C1_1,     LeftMotor,     tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     RightMotor,    tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    Flipper,              tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma debuggerWindows("joystickGame");

#include "JoystickDriver.c"

int DistanceToIr;

task main()
{
	int deadZone = 15;
	waitForStart();

	while (SensorValue[IRseeker] != 9)
	{
		motor[LeftMotor] = 50;											//forward until /ir = 7
		motor[RightMotor] = 50;
	}

	DistanceToIr = nMotorEncoder[LeftMotor];        //get value

	motor[LeftMotor] = 0;													//stop motors
	motor[RightMotor] = 0;

	nMotorEncoder[LeftMotor] = 0;				//reset encoders
	nMotorEncoder[RightMotor] = 0;

	while (nMotorEncoder[LeftMotor] > -50)			//2 rotations on motorB for a turn, hopefully hopefully a 90 degree turn, troubleshoot later
	{
		motor[LeftMotor] = -75;
		motor[RightMotor] = 75;
	}

	motor[LeftMotor] = 0;															//stop motor
	motor[RightMotor] = 0;
	nMotorEncoder[RightMotor] = 0;
	nMotorEncoder[LeftMotor] = 0;							//reset encoder

	servo[Flipper] = 50;                      //drop block into basket

	while (nMotorEncoder[LeftMotor] < 50)
	{
		motor[LeftMotor] = 75;									//turn back
		motor[RightMotor] = -75;
	}

	motor[LeftMotor] = 0;															//stop motor
	motor[RightMotor] = 0;
	nMotorEncoder[RightMotor] = 0;
	nMotorEncoder[LeftMotor] = 0;							//reset encoder

	while (nMotorEncoder[LeftMotor] > -DistanceToIr)				//back up
	{
		motor[LeftMotor] = -50;
		motor[RightMotor] = -50;
	}																///////////////////done backing up

	while (nMotorEncoder[LeftMotor] > -2800)			//turning towards ramp
	{
		motor[LeftMotor] = -75;
		motor[RightMotor] = 75;
	}																										//done turning

	motor[LeftMotor] = 0;															//stop motor
	motor[RightMotor] = 0;
	nMotorEncoder[RightMotor] = 0;
	nMotorEncoder[LeftMotor] = 0;							//reset encoder

	while (nMotorEncoder[LeftMotor] < 4000)			//travel to ramp hopefully
	{
		motor[LeftMotor] = 50;
		motor[RightMotor] = 50;
	}																						//arrives at ramp

	motor[LeftMotor] = 0;															//stop motor
	motor[RightMotor] = 0;
	nMotorEncoder[RightMotor] = 0;
	nMotorEncoder[LeftMotor] = 0;							//reset encoder

	while (nMotorEncoder[RightMotor] > - 2800)				//turn
	{
		motor[RightMotor] = -75;
		motor[LeftMotor] = 75;
	}																							//done turning

	motor[LeftMotor] = 0;															//stop motor
	motor[RightMotor] = 0;
	nMotorEncoder[RightMotor] = 0;
	nMotorEncoder[LeftMotor] = 0;							//reset encoder

	while (nMotorEncoder[LeftMotor] < 4000)				//go up ramp
	{
		motor[LeftMotor] = 75;
		motor[RightMotor] = 75;
	}																							//at top of ramp

	motor[LeftMotor] = 0;															//stop motor
	motor[RightMotor] = 0;
	nMotorEncoder[RightMotor] = 0;
	nMotorEncoder[LeftMotor] = 0;							//reset encoder
}
